package autonomous;

import genetic.DNA;

import java.util.ArrayList;

import processing.core.PApplet;
import processing.core.PVector;

public class FlockBoid extends PApplet {
	// Flocking
	// Daniel Shiffman <http://www.shiffman.net>
	// The Nature of Code, Spring 2009

	// Boid class
	// Methods for Separation, Cohesion, Alignment added

	/**
	 * 
	 */
	private static final long serialVersionUID = 1L;
	public PVector loc;
	PVector vel;
	PVector acc;
	float r;
	float maxforce; // Maximum steering force
	float maxspeed; // Maximum speed
	PApplet parent; //
	DNA genes;
	int finish; // what was my finish? (first, second, etc. . . )
	float fitness;
//	float timer;
	float recordDist;
	int diam = 24; // size of target

	public FlockBoid(PApplet p, PVector l, DNA dna, float ms, float mf) {
		parent = p;
		recordDist = parent.width;
		acc = new PVector(0, 0);
		vel = new PVector(random(-1, 1), random(-1, 1));
		loc = l.get();
		r = (float) 2.0;
		maxspeed = ms;
		maxforce = mf;
		genes = dna;
	}

	void run(ArrayList<FlockBoid> boids) {
		flock(boids);
		update();
		borders();
		render();
	}

	// We accumulate a new acceleration each time based on three rules
	void flock(ArrayList<FlockBoid> boids) {
		PVector sep = separate(boids); // Separation
		PVector ali = align(boids); // Alignment
		PVector coh = cohesion(boids); // Cohesion
		// Arbitrarily weight these forces
		sep.mult((float) 1.5);
		ali.mult((float) 1.0);
		coh.mult((float) 1.0);
		// Add the force vectors to acceleration
		acc.add(sep);
		acc.add(ali);
		acc.add(coh);
	}

	// Method to update location
	void update() {
		// Update velocity
		vel.add(acc);
		// Limit speed
		vel.limit(maxspeed);
		loc.add(vel);
		// Reset accelertion to 0 each cycle
		acc.mult(0);
	}

	void seek(PVector target) {
		acc.add(steer(target, false));
	}

	void arrive(PVector target) {
		acc.add(steer(target, true));
	}

	public void applyForce(PVector force) {
		float mass = 10; // We aren't bothering with mass here
		force.div(mass);
		acc.add(force);
	}

	// A method that calculates a steering vector towards a target
	// Takes a second argument, if true, it slows down as it approaches the
	// target
	PVector steer(PVector target, boolean slowdown) {
		PVector steer; // The steering vector
		PVector desired = PVector.sub(target, loc); // A vector pointing from
													// the location to the
													// target
		float d = desired.mag(); // Distance from the target is the magnitude of
									// the vector
		// If the distance is greater than 0, calc steering (otherwise return
		// zero vector)
		if (d > 0) {
			// Normalize desired
			desired.normalize();
			// Two options for desired vector magnitude (1 -- based on distance,
			// 2 -- maxspeed)
			if ((slowdown) && (d < 10.0f))
				desired.mult(maxspeed * (d / 100.0f)); // This damping is
														// somewhat arbitrary
			else
				desired.mult(maxspeed);
			// Steering = Desired minus Velocity
			steer = PVector.sub(desired, vel);
			steer.limit(maxforce); // Limit to maximum steering force
		} else {
			steer = new PVector(0, 0);
		}
		return steer;
	}

	void render() {
		// Draw a triangle rotated in the direction of velocity
		float theta = vel.heading2D() + radians(90);
		parent.fill(175);
		parent.stroke(0);
		parent.pushMatrix();
		parent.translate(loc.x, loc.y);
		parent.rotate(theta);
		parent.beginShape(TRIANGLES);

		parent.vertex(r * 5, -r * 2);
		parent.vertex(-5 * r, r * 20);
		parent.vertex(r * 5, -r * 12);

		parent.endShape();
		parent.popMatrix();
	}

	// Wraparound
	void borders() {
		if (loc.x < -r)
			loc.x = parent.width + r;
		if (loc.y < -r)
			loc.y = parent.height + r;
		if (loc.x > parent.width + r)
			loc.x = -r;
		if (loc.y > parent.height + r)
			loc.y = -r;
	}

	// Separation
	// Method checks for nearby boids and steers away
	PVector separate(ArrayList<FlockBoid> boids) {
		float desiredseparation = (float) 25.0;
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		// For every boid in the system, check if it's too close
		for (int i = 0; i < boids.size(); i++) {
			FlockBoid other = (FlockBoid) boids.get(i);
			float d = PVector.dist(loc, other.loc);
			// If the distance is greater than 0 and less than an arbitrary
			// amount (0 when you are yourself)
			if ((d > 0) && (d < desiredseparation)) {
				// Calculate vector pointing away from neighbor
				PVector diff = PVector.sub(loc, other.loc);
				diff.normalize();
				diff.div(d); // Weight by distance
				steer.add(diff);
				count++; // Keep track of how many
			}
		}
		// Average -- divide by how many
		if (count > 0) {
			steer.div((float) count);
		}

		// As long as the vector is greater than 0
		if (steer.mag() > 0) {
			// Implement Reynolds: Steering = Desired - Velocity
			steer.normalize();
			steer.mult(maxspeed);
			steer.sub(vel);
			steer.limit(maxforce);
		}
		return steer;
	}

	// Alignment
	// For every nearby boid in the system, calculate the average velocity
	PVector align(ArrayList<FlockBoid> boids) {
		float neighbordist = (float) 50.0;
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		for (int i = 0; i < boids.size(); i++) {
			FlockBoid other = (FlockBoid) boids.get(i);
			float d = PVector.dist(loc, other.loc);
			if ((d > 0) && (d < neighbordist)) {
				steer.add(other.vel);
				count++;
			}
		}
		if (count > 0) {
			steer.div((float) count);
		}

		// As long as the vector is greater than 0
		if (steer.mag() > 0) {
			// Implement Reynolds: Steering = Desired - Velocity
			steer.normalize();
			steer.mult(maxspeed);
			steer.sub(vel);
			steer.limit(maxforce);
		}
		return steer;
	}

	// Cohesion
	// For the average location (i.e. center) of all nearby boids, calculate
	// steering vector towards that location
	PVector cohesion(ArrayList<FlockBoid> boids) {
		float neighbordist = (float) 50.0;
		PVector sum = new PVector(0, 0, 0); // Start with empty vector to
											// accumulate all locations
		int count = 0;
		for (int i = 0; i < boids.size(); i++) {
			FlockBoid other = (FlockBoid) boids.get(i);
			float d = PVector.dist(loc, other.loc);
			if ((d > 0) && (d < neighbordist)) {
				sum.add(other.loc); // Add location
				count++;
			}
		}
		if (count > 0) {
			sum.div((float) count);
			return steer(sum, false); // Steer towards the location
		}
		return sum;
	}

	float calcFitness() {
		// float d = recordDist; // PVector.distance(loc,target);
		// if (d < diam / 2) {
		// d = 1.0f;
		// }
		// // reward finishing faster and getting closer
		// fitness = (1.0f / pow(finish, 1.5f)) * (1.0f / (pow(d, 6)));

		fitness  =1;
		
		return fitness;
	}

	public float getFitness() {

		fitness = calcFitness();
		return fitness;
	}

	// boolean finished(PVector target) {
	// float d = dist(loc.x, loc.y, target.r.x, target.r.y);
	// if (d < recordDist)
	// recordDist = d;
	// // if ( d < diam/2) {
	// if (target.contains(loc)) {
	// stopped = true;
	// return true;
	// }
	// return false;
	// }

	void setFinish(int f) {
		finish = f;
		// print(finish + " ");
	}

	public DNA getGenes() {
		return genes;
	}

}
